{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# CommonRoad Tutorial: Reading, Modifying, and Writing XML files\n",
    "\n",
    "This tutorial shows how CommonRoad XML-files can be read, modified, visualized, and stored. To start with, a CommonRoad XML-file consists of a **Scenario** and a **PlanningProblemSet**:\n",
    "* A **Scenario** represents the environment including a **LaneletNetwork** and a set of **DynamicObstacle** and **StaticObstacle**.\n",
    "    * A **LaneletNetwork** is built from lane segments (**Lanelet**), that can be connected arbitrarily.\n",
    "* A **PlanningProblemSet** contains one **PlanningProblem** for every ego vehicle in the **Scenario**, consisting of an **initial position** and a **GoalRegion** that has to be reached."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0. Preparation\n",
    "* Before you proceed any further, make sure you have skimmed through [CommonRoad API](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html#modules) to gain an overall view of the funtionalities provided by CommonRoad modules. You may need to refer to it for implementation details throughout this tutorial.\n",
    "\n",
    "* Additional documentations on **CommonRoad XML format, Cost Functions, Vehicle Models, etc.** can be found at [CommonRoad](https://commonroad.in.tum.de/) under section **Documentation**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Read XML file\n",
    "\n",
    "As documented in [CommonRoadFileReader](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.file_reader), the **CommonRoadFileReader** reads in a CommonRoad XML file, and its **open()** method returns a **Scenario** and a **PlanningProblemSet** object:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 1440x720 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "time: 11.7 s\n"
     ]
    }
   ],
   "source": [
    "import os\n",
    "import matplotlib.pyplot as plt\n",
    "from IPython import display\n",
    "\n",
    "# import functions to read xml file and visualize commonroad objects\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "from commonroad.visualization.draw_dispatch_cr import draw_object\n",
    "\n",
    "# generate path of the file to be opened\n",
    "file_path = os.path.join(os.getcwd(),\"ZAM_Tutorial-1_1_T-1.xml\")\n",
    "\n",
    "# read in the scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(file_path).open()\n",
    "\n",
    "# plot the scenario for 40 time step, here each time step corresponds to 0.1 second\n",
    "for i in range(0, 40):\n",
    "    # clear previous graph\n",
    "    display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(20, 10))\n",
    "    # plot the scenario at different time step\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    # plot the planning problem set\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Modify XML file\n",
    "\n",
    "It is possible to modify existing CommonRoad scenarios to costumize them to one's need. First, we would like to add a static obstacle to the scenario with the following specification:\n",
    "\n",
    "    - obstacle type: parked vehicle\n",
    "    - obstacle shape: rectangle, with a width of 2.0m and a length of 4.5m\n",
    "    - initial state:\n",
    "        - position: (30, 3.5) m\n",
    "        - orientation: 0.02 rad\n",
    "* obstacle id: since every object in the scenario must have a unique ID, we can use the member function **generate_object_id** of **Scenario** class to generate a unique ID for the object.\n",
    "\n",
    "As documented in [StaticObstacle](https://commonroad.in.tum.de/static/docs/commonroad-io/api/scenario.html#commonroad.scenario.obstacle.StaticObstacle), we need to provide `obstacle_id, obstacle_type, obstacle_shape, initial_state` to construct a static obstacle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAABY4AAABpCAYAAACd67dwAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjEsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy8QZhcZAAAPjklEQVR4nO3da4yc110G8Oc/M+utYzskVkLoxSEFtUh1Imidppa4C+iFoAb4gFoQlIsUkNoKhMT9A6gS4o4EFQKCqFQk2kIFERUqvSAQ/VRIHCqSlBbcNFWcpg2J1dJdu+vdmcOHHZcdd9fZJDPz7o5/P2k1Myez8z4bHZ2Zffzueau1FgAAAAAAuKjXdQAAAAAAAPYWxTEAAAAAABMUxwAAAAAATFAcAwAAAAAwQXEMAAAAAMAExTEAAAAAABMGs3jR6667rt10002zeGkAAAAAAKbk1KlTT7TWrr90fCbF8U033ZR77713Fi8NAAAAAMCUVNWnthu3VQUAAAAAABMUxwAAAAAATJjJVhVXsne/+9155JFHuo4BAAAAAOzCq171qhw/frzrGHuO4njKjh07lmPHjnUdAwAAAADYBaXx9mxVAQAAAADABMUxAAAAAAATFMcAAAAAAExQHE/RcDhMr+d/KQAAAACwv2k5p2htbS1Hjx7N1VdfrUAGAAAAAPatQdcBFk2v18tznvOcLC0tZX19PSsrKxmNRl3HAgAAAADYNcXxjPT7/fT7fQUyAAAAALDvKI5nTIEMAAAAAOw3iuM5uVggHzhwIF/84hezsrLSdSQAAAAAgG0pjudoNBplOBymtZZDhw51HQcAAAAArnjnz5/PwYMHu46x5yiO52BjYyMXLlzI6upqWmtdxwEAAAAAxpTG21Mcz5DCGAAAAADYjxTHM6AwBgAAAAD2M8XxFPX7/Zw7d05hDAAAAADsa4rjKVpeXs7KykrXMQAAAAAAnpVe1wEAAAAAANhbFMcAAAAAAEzY1VYVVfVwki8kGSbZaK3dOstQAAAAAAB05+nscfztrbUnZpYEAAAAAIA9wVYVAAAAAABM2O0Zxy3JB6qqJfnT1tpdM8y0r506dSqnT5/uOgYAAAAAsAtPPvlkbr/99q5j7Dm7LY6/qbX2aFV9ZZIPVtXHWmsf2vqEqrozyZ1JcuONN0455v5x4sSJnDhxousYAAAAAMAunDx5susIe9KutqporT06vn08yd1JbtvmOXe11m5trd16/fXXTzclAAAAAABz85TFcVUdqqojF+8neWWSB2YdDAAAAACAbuxmq4obktxdVRef/47W2vtmmgrYs9polDYcZjQcpm1sbLk/TBtuZHRxbGOYNhyPrW+kra9/6Wu0vp5sbI5nNMoNr35l+gcOdP2jAQAAADD2lMVxa+2hJF8/hyz7Xmut6wjwrIzW1/PxX/+NHF5eTlpLWku1jO+PNm9HLb2q9JL0KqlU+kkq+dJtL8mgtc3ntPHzq9Lr9VIX71elqnLm3GrWbnt5rvqqG7r7wQEAAACYsNuL47EL58+fz9GjR7O6upq1tbWu48Azck0qx5efM7fjLa+tpQ035nY8AAAAAJ7ari6Ox+4NBoMcOXIk1157bZaXl7uOA09L9fsZzfmYvSSjDcUxAAAAwF6iOJ6BXq+XpaUlBTL7TvV6mfeGK/2qtOFwzkcFAAAA4HIUxzOkQGY/apsXwpybai1tQ3EMAAAAsJcojufg0gL5wIEDXUeCnc25OO61lpEzjgEAAAD2FBfHm6OqSq/Xy/LycpaWlrqOA9vq9fvzPV5VlgeDHDp0aK7HBQAAAEiS8+fP5+DBg13H2HMUx3MwGo0yHA5z7ty5rK2tdR0HLms0512Oe1U5/4UvZHV1da7HBQAAAEiiNN6B4niGFMbsS/PeqqIqo/X1uR4TAAAAgMtTHM+Awph9bd4Xx6tK1jfmekwAAAAALk9xPGUbGxtZXV1VGLNvtS7OON5wxjEAAADAXqI4nqKrrroqZ8+e7ToGPDsdFMfNVhUAAAAAe4riGJgwGrWsrq2ltZbRxa/RaPJxVUbJ+LZllPHjbF5cryWbz0tl1FpStXkmc682i+nxV6tehq3l6qu/otsfGgAAAIAJimNgwuFv/9Z88okn01sapJaWUoOl1MX7S0ub4/1BeoN+qj9IDfpZ6v///V6/n+r30xsMUoNBqtdL9Xpd/1gAAAAAPA2KY2DCdSdf0XUEAAAAADrmNEAAAAAAACYojgEAAAAAmGCriik7depUTp8+3XUMAAAAAGAXnnzyydx+++1dx9hzFMdTduLEiZw4caLrGAAAAADALpw8ebLrCHuSrSoAAAAAAJjgjGMAAJ6W3/nwgXx6ZX4fI593eCM/d/LC3I4HAAAojgEAeJo+vTLIo+vPnd8BVx5LojgGAIB5slUFAAAAAAATFMdTtLa2lsOHD6equo4CAAAAAPCM7ao4rqpXV9XHq+p0Vf3irEPtV8PhMFdddVWuvfZaBTIAAAAAsG89ZXFcVf0kf5TkNUlekuT1VfWSWQfbzwaDgQIZAGCLjc+fyXDl8Wf0vStnHphyGgAA4Kns5ozj25Kcbq091Fq7kORdSe6YbazFoECer/vv/1ze/vaHcv/9n+s6CgBwidEXP5/R2soz+t4Ln/9MWmtTTgQAAFzOYBfPeX6SR7Y8PpPkFbOJs5gGg0EGg0EOHDiQCxcuZHV11S8/U3b//Z/Lm998T9bXR1la6uWtb315brnlmq5jAQBjB244/oy/9+jx70zVuSmmAQAAnspuiuNdqao7k9yZJDfeeOO0XnahDAaD9Hq9DAaDrK+vdx1noTzwwJmsr48yGiUbG6M88MBKTp58ftexAGAh9fu9ZI4fZfr9fg4dOjS/AwIAcEU5f/58Dh482HWMPWc3xfGjSY5tefyC8diE1tpdSe5KkltvvdXptJcYDodZX1/PyspKRqNR13EWzs03H87SUi/r66MMBr3cfPPhrK6udh0LABbScHjVnI83zOqqM44BAJgNpfH2dlMc35PkRVX1wmwWxq9L8oMzTbVAFMbzccst1+Stb3157rvvbF72sqO2qQAAAACAZ+Epi+PW2kZVvSnJ+5P0k7yttfbgzJPtcwrj+bvllmsUxgAAAAAwBbva47i19t4k751xloUwGo1y4cIFhTEAsLCed3gjWXlsvscDAADmamoXxyNZXl7O2bNnFcYAwEL7uZMXklzoOgYAADBDva4DLJJ+v680BgAAAAD2PcUxAAAAAAATFMcAAAAAAExQHAMAAAAAMEFxDAAAAADAhEHXARbNkSNH8uCDD3YdAwAAAADYhSNHjuT48eNdx9hzFMdTdvz4cRMNAAAAANjXbFUBAAAAAMAExTEAAAAAABOqtTb9F636nySfmvoL7w/XJXmi6xBckcw9umT+0RVzj66Ye3TJ/KMr5h5dMffo0pUw/766tXb9pYMzKY6vZFV1b2vt1q5zcOUx9+iS+UdXzD26Yu7RJfOPrph7dMXco0tX8vyzVQUAAAAAABMUxwAAAAAATFAcT99dXQfgimXu0SXzj66Ye3TF3KNL5h9dMffoirlHl67Y+WePYwAAAAAAJjjjGAAAAACACYrjKamqV1fVx6vqdFX9Ytd5WGxVdayq/rmqPlpVD1bVT4/Hf62qHq2qj4y/vrvrrCyeqnq4qu4fz7F7x2NHq+qDVfXf49tru87JYqmqr9uytn2kqv63qn7GusesVNXbqurxqnpgy9i2a11t+sPx58D/qKqXdZec/W6Hufc7VfWx8fy6u6quGY/fVFXnt6yBf9JdchbBDvNvx/faqvql8dr38ap6VTepWQQ7zL2/2jLvHq6qj4zHrX1MzWX6FZ/7YquKqaiqfpL/SvJdSc4kuSfJ61trH+00GAurqp6b5Lmttfuq6kiSU0m+N8kPJFlprf1upwFZaFX1cJJbW2tPbBn77SRnW2u/Of7Hs2tba7/QVUYW2/h999Ekr0jyY7HuMQNV9S1JVpL8RWvt5vHYtmvduER5c5Lvzua8/IPW2iu6ys7+tsPce2WSf2qtbVTVbyXJeO7dlOTvLz4Pnq0d5t+vZZv32qp6SZJ3JrktyfOS/GOSF7fWhnMNzULYbu5d8t9/L8nnW2tvsfYxTZfpV340Pvc543hKbktyurX2UGvtQpJ3Jbmj40wssNbaY621+8b3v5DkP5M8v9tUXOHuSPL28f23Z/ONFmblO5J8orX2qa6DsLhaax9KcvaS4Z3Wujuy+Ytua619OMk1419C4Gnbbu611j7QWtsYP/xwkhfMPRhXhB3Wvp3ckeRdrbW11tonk5zO5u/G8LRdbu5VVWXzJKl3zjUUV4TL9Cs+90VxPC3PT/LIlsdnosRjTsb/2vrSJP86HnrT+M8l3ma7AGakJflAVZ2qqjvHYze01h4b3/9Mkhu6icYV4nWZ/MXBuse87LTW+SzIPP14kn/Y8viFVfXvVfUvVfXNXYVi4W33XmvtY16+OclnW2v/vWXM2sfUXdKv+NwXxTHsa1V1OMnfJPmZ1tr/JvnjJF+b5BuSPJbk9zqMx+L6ptbay5K8Jskbx39W9iVtcw8k+yAxE1V1IMlrk7x7PGTdoxPWOrpQVb+SZCPJX46HHktyY2vtpUl+Nsk7qurqrvKxsLzX0rXXZ/KkAWsfU7dNv/IlV/LnPsXxdDya5NiWxy8Yj8HMVNVSNhe1v2yt/W2StNY+21obttZGSf4s/lSMGWitPTq+fTzJ3dmcZ5+9+Oc549vHu0vIgntNkvtaa59NrHvM3U5rnc+CzFxV/WiS70nyQ+NfYDPeIuDJ8f1TST6R5MWdhWQhXea91trHzFXVIMn3J/mri2PWPqZtu34lPvclURxPyz1JXlRVLxyfCfW6JO/pOBMLbLzH058n+c/W2u9vGd+6r873JXng0u+FZ6OqDo0vGJCqOpTkldmcZ+9J8obx096Q5O+6ScgVYOKME+sec7bTWveeJD8yvsr2yWxevOex7V4AnomqenWSn0/y2tbauS3j148vGJqq+pokL0ryUDcpWVSXea99T5LXVdVyVb0wm/Pv3+adj4X3nUk+1lo7c3HA2sc07dSvxOe+JMmg6wCLYHx14zcleX+SfpK3tdYe7DgWi+0bk/xwkvur6iPjsV9O8vqq+oZs/gnFw0l+spt4LLAbkty9+d6aQZJ3tNbeV1X3JPnrqvqJJJ/K5sUrYKrG/1jxXZlc237buscsVNU7k3xbkuuq6kySX03ym9l+rXtvNq+sfTrJuSQ/NvfALIwd5t4vJVlO8sHxe/CHW2s/leRbkrylqtaTjJL8VGtttxc2gy+zw/z7tu3ea1trD1bVXyf5aDa3UHlja23YRW72v+3mXmvtz/Pl17ZIrH1M1079is99SWr8V04AAAAAAJDEVhUAAAAAAFxCcQwAAAAAwATFMQAAAAAAExTHAAAAAABMUBwDAAAAADBBcQwAAAAAwATFMQAAAAAAExTHAAAAAABM+D/uZFtFqed8ggAAAABJRU5ErkJggg==\n",
      "text/plain": [
       "<Figure size 1800x720 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "\n",
    "# import necesary classes from different modules\n",
    "from commonroad.geometry.shape import Rectangle\n",
    "from commonroad.scenario.obstacle import StaticObstacle, ObstacleType\n",
    "from commonroad.scenario.trajectory import State\n",
    "\n",
    "# read in the scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(file_path).open()\n",
    "\n",
    "# generate the static obstacle according to the specification, refer to API for details of input parameters\n",
    "static_obstacle_id = scenario.generate_object_id()\n",
    "static_obstacle_type = ObstacleType.PARKED_VEHICLE\n",
    "static_obstacle_shape = Rectangle(width = 2.0, length = 4.5)\n",
    "static_obstacle_initial_state = State(position = np.array([30.0, 3.5]), orientation = 0.02, time_step = 0)\n",
    "\n",
    "# feed in the required components to construct a static obstacle\n",
    "static_obstacle = StaticObstacle(static_obstacle_id, static_obstacle_type, static_obstacle_shape, static_obstacle_initial_state)\n",
    "\n",
    "# add the static obstacle to the scenario\n",
    "scenario.add_objects(static_obstacle)\n",
    "\n",
    "# plot the scenario for each time step\n",
    "for i in range(0, 40):\n",
    "    display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(25, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As can be seen, we have added a new static obstacle to the scenario. We can further add a dynamic obstacle with the following specifications:\n",
    "\n",
    "    - obstacle type: car\n",
    "    - obstacle shape: rectangle with a width of 1.8m and a length of 4.3m\n",
    "    - initial state:\n",
    "        - position: (50, 0.0) m\n",
    "        - orientation: 0.00 rad\n",
    "        - velocity: 22 m/s along x axis\n",
    "    - we assume that the dynamic obstacle drives with constant velocity.\n",
    "\n",
    "As documented in [DynamicObstacle](https://commonroad.in.tum.de/static/docs/commonroad-io/api/scenario.html#commonroad.scenario.obstacle.DynamicObstacle), we need to pass in a **Prediction** object which in this case is a **TrajectoryPrediction** object. Its generation goes as follows:\n",
    "    1. compute all subsequent states for the dynamic obstacle\n",
    "    2. create a Trajectory from these states\n",
    "    3. create a TrajectoryPrediction from this trajectory and obstacle shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 1800x720 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# import necesary classes from different modules\n",
    "from commonroad.scenario.obstacle import DynamicObstacle\n",
    "from commonroad.scenario.trajectory import Trajectory\n",
    "from commonroad.prediction.prediction import TrajectoryPrediction\n",
    "\n",
    "dynamic_obstacle_id = scenario.generate_object_id()\n",
    "dynamic_obstacle_type = ObstacleType.CAR\n",
    "# initial state has a time step of 0\n",
    "dynamic_obstacle_initial_state = State(position = np.array([50.0, 0.0]),\n",
    "                                       velocity = 22,\n",
    "                                       orientation = 0.02,\n",
    "                                       time_step = 0)\n",
    "\n",
    "# generate the states for the obstacle for time steps 1 to 40 by assuming constant velocity\n",
    "state_list = []\n",
    "for i in range(1, 40):\n",
    "    # compute new position\n",
    "    new_position = np.array([dynamic_obstacle_initial_state.position[0] + scenario.dt * i * 22, 0])\n",
    "    # create new state\n",
    "    new_state = State(position = new_position, velocity = 22,orientation = 0.02, time_step = i)\n",
    "    # add new state to state_list\n",
    "    state_list.append(new_state)\n",
    "\n",
    "# create the trajectory of the obstacle, starting at time step 1\n",
    "dynamic_obstacle_trajectory = Trajectory(1, state_list)\n",
    "\n",
    "# create the prediction using the trajectory and the shape of the obstacle\n",
    "dynamic_obstacle_shape = Rectangle(width = 1.8, length = 4.3)\n",
    "dynamic_obstacle_prediction = TrajectoryPrediction(dynamic_obstacle_trajectory, dynamic_obstacle_shape)\n",
    "\n",
    "# generate the dynamic obstacle according to the specification\n",
    "dynamic_obstacle_id = scenario.generate_object_id()\n",
    "dynamic_obstacle_type = ObstacleType.CAR\n",
    "dynamic_obstacle = DynamicObstacle(dynamic_obstacle_id, \n",
    "                                   dynamic_obstacle_type, \n",
    "                                   dynamic_obstacle_shape, \n",
    "                                   dynamic_obstacle_initial_state, \n",
    "                                   dynamic_obstacle_prediction)\n",
    "\n",
    "# add dynamic obstacle to the scenario\n",
    "scenario.add_objects(dynamic_obstacle)\n",
    "\n",
    "# plot the scenario for each time step\n",
    "for i in range(0, 40):\n",
    "    display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(25, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Write XML file\n",
    "After we have modified the scenario, we would like to save the changes and write the **Scenario** and **PlanningProblemSet** to a CommonRoad XML file. [CommonRoadFileWriter](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.file_writer) helps us with this purpse.\n",
    "Note that we did not modify the **PlanningProblemSet** in this tutorial."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import necesary classes from different modules\n",
    "from commonroad.common.file_writer import CommonRoadFileWriter\n",
    "from commonroad.common.file_writer import OverwriteExistingFile\n",
    "\n",
    "author = 'Max Mustermann'\n",
    "affiliation = 'Technical University of Munich, Germany'\n",
    "source = ''\n",
    "tags = 'highway multi_lane no_oncoming_traffic parallel_lanes comfort'\n",
    "\n",
    "# write new scenario\n",
    "fw = CommonRoadFileWriter(scenario, planning_problem_set, author, affiliation, source, tags)\n",
    "\n",
    "filename = \"ZAM_Tutorial-1_2_T-1.xml\"\n",
    "fw.write_to_file(filename, OverwriteExistingFile.ALWAYS)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can open our stored file again to check if everything is correct:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAABY4AAABpCAYAAACd67dwAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjEsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy8QZhcZAAAQ+klEQVR4nO3df4yk930X8PdnZnY357tz7ZON6zg2DihFytmi9TnOif4WbX7UqAb+QAkI2oJkKjURFaLQwh+tKiFKS5EgQgWjRgpSk7QRWFgotAkC0T8gYJ8J2E4TcnUd+RzXxj4l9e6d98fMlz92zt257p73zjPz7M69XvLqeeZ7M/N81vrq+zzz3me+32qtBQAAAAAALul1XQAAAAAAAAeL4BgAAAAAgAmCYwAAAAAAJgiOAQAAAACYIDgGAAAAAGCC4BgAAAAAgAmDWbzpLbfc0u6+++5ZvDUAAAAAAFNy5syZV1prt17ePpPg+O67784TTzwxi7cGAAAAAGBKqupru7WbqgIAAAAAgAmCYwAAAAAAJsxkqorr2Wc+85k8//zzXZcBAAAAAOzD+9///pw8ebLrMg4cwfGU3Xnnnbnzzju7LgMAAAAA2Aeh8e5MVQEAAAAAwATBMQAAAAAAEwTHAAAAAABMEBxP0XA4TK/nfykAAAAAcLhJOadofX09J06cyI033ihABgAAAAAOrUHXBSyaXq+Xt73tbVlaWsrm5mZWV1czGo26LgsAAAAAYN8ExzPS7/fT7/cFyAAAAADAoSM4njEBMgAAAABw2AiO5+RSgLy8vJzXX389q6urXZcEAAAAALArwfEcjUajDIfDtNZy9OjRrssBAAAAgOvexYsXc+TIka7LOHAEx3OwtbWVjY2NrK2tpbXWdTkAAAAAwJjQeHeC4xkSGAMAAAAAh5HgeAYExgAAAADAYSY4nqJ+v58LFy4IjAEAAACAQ01wPEUrKytZXV3tugwAAAAAgLek13UBAAAAAAAcLIJjAAAAAAAm7Guqiqp6LslrSYZJtlpr98+yKAAAAAAAunM1cxx/f2vtlZlVAgAAAADAgWCqCgAAAAAAJuz3juOW5HNV1ZL8q9baIzOs6VA7c+ZMzp4923UZAAAAAMA+vPrqq3nwwQe7LuPA2W9w/F2ttReq6o8l+XxVfbm19ts7n1BVDyd5OEnuuuuuKZd5eJw6dSqnTp3qugwAAAAAYB9Onz7ddQkH0r6mqmitvTDevpzk0SQP7PKcR1pr97fW7r/11lunWyUAAAAAAHPzpsFxVR2tquOX9pO8L8nTsy4MAAAAAIBu7GeqituSPFpVl57/ydbab860KuDAaqNR2nCY0XCYtrW1Y3+YNtzK6FLb1jBtOG7b3Erb3HzjZ7S5mWxtt2c0ym0feF/6y8td/2oAAAAAjL1pcNxaezbJn55DLYdea63rEuAtGW1u5iv/8B/l2MpK0lrSWqplvD/a3o5aelXpJelVUqn0k1TyxraXZNDa9nPa+PlV6fV6qUv7VamqnLuwlvUH3pMbvvW27n5xAAAAACbsd3E89uHixYs5ceJE1tbWsr6+3nU5cE1uSuXkytvmdryV9fW04dbcjgcAAADAm9vX4njs32AwyPHjx3PzzTdnZWWl63LgqlS/n9Gcj9lLMtoSHAMAAAAcJILjGej1ellaWhIgc+hUr5d5T7jSr0obDud8VAAAAACuRHA8QwJkDqO2vRDm3FRraVuCYwAAAICDRHA8B5cHyMvLy12XBHubc3Dcay0jdxwDAAAAHCgWx5ujqkqv18vKykqWlpa6Lgd21ev353u8qqwMBjl69OhcjwsAAACQJBcvXsyRI0e6LuPAERzPwWg0ynA4zIULF7K+vt51OXBFoznPctyrysXXXsva2tpcjwsAAACQRGi8B8HxDAmMOZTmPVVFVUabm3M9JgAAAABXJjieAYExh9q8F8erSja35npMAAAAAK5McDxlW1tbWVtbExhzaLUu7jjecscxAAAAwEEiOJ6iG264IefPn++6DHhrOgiOm6kqAAAAAA4UwTEwYTRqWVtfT2sto0s/o9Hk46qMkvG2ZZTx42wvrteS7eelMmotqdq+k7lX28H0+KdVL8PWcuON39LtLw0AAADABMExMOHY939vfu+VV9NbGqSWllKDpdSl/aWl7fb+IL1BP9UfpAb9LPX/cL/X76f6/fQGg9RgkOr1Ur1e178WAAAAAFdBcAxMuOX0e7suAQAAAICOuQ0QAAAAAIAJgmMAAAAAACaYqmLKzpw5k7Nnz3ZdBgAAAACwD6+++moefPDBrss4cATHU3bq1KmcOnWq6zIAAAAAgH04ffp01yUcSKaqAAAAAABggjuOAQC4Kr/0heV8fXV+l5FvP7aVnzq9MbfjAQAAgmMAAK7S11cHeWHz9vkdcPXFJIJjAACYJ8ExAAAAnftv53r53y/1szVKNkaVrVGyNUpaS0aptJYMWzJqlZZxe6uMWtKyvR3teO5ox3Mn9ytrG6P87Heu5d23to5/awA4uATHU7S+vp5jx45lbW0trbkAAQAA2K/Hvrqcr1741lQqVZUkb2ynqpL1ej2vbaxN/70BYIHsa3G8qvpAVX2lqs5W1U/PuqjDajgc5oYbbsjNN9+cY8eOzeYiBwAAYAENekm/10+v10tVzfjzVGV96PMaAFzJmwbHVdVP8i+SfDDJu5N8uKrePevCDrPBYCBABgDYYeub5zJcffmaXrt67ukpVwMcRL05fmyqqmwM53c8ADiM9nPH8QNJzrbWnm2tbST5dJKHZlvWYhAgz9dTT30jn/jEs3nqqW90XQoAcJnR69/MaH31ml678c3fNw0YXAfmGRwnyabgGACuaD9zHN+R5Pkdj88lee9syllMg8Egg8Egy8vL2djYMAfyDDz11Dfy0Y8+ns3NUZaWevnYx96Te++9qeuyAICx5dtOXvNrT5z8gVRdmGI1wEG0vXTd1RttrKW3fPTqjlWmqgCANzO1xfGq6uEkDyfJXXfdNa23XSiDwSC9Xi+DwSCbm5tdl7NQnn76XDY3RxmNkq2tUZ5+ejWnT9/RdVkAsJD6/V4yx0uZfr+fo0evLhQCDp+lpWt73Wv//VfyLd/7d676dTVYydGjwmMAkosXL+bIkSNdl3Hg7Cc4fiHJnTsev2PcNqG19kiSR5Lk/vvvdzvtZYbDYTY3N7O6uprRaNR1OQvnnnuOZWmpl83NUQaDXu6551jW1qySDACzMBzeMOfjDbO25o5jWHSjreVret01hcZVWbu4kbW1rWs6JgCLRWi8u/0Ex48neVdVvTPbgfGHkvzlmVa1QATG83HvvTflYx97T5588nzuu++EaSoAAOCQ6VXSWpvL2jCVyoapKgDgit40OG6tbVXVR5L8VpJ+ko+31p6ZeWWHnMB4/u699yaBMQAAHFKD/SzdPi2VrPuYBgBXtK85jltrn03y2RnXshBGo1E2NjYExgDAwnr7sa1k9cX5Hg9YeMv9pKWlMp87jjeHMz8MABxqU1scj2RlZSXnz58XGAMAC+2nTm8k2ei6DGDBDHotaUnGU1Yk20Hy9n87ty0tO57TWjJur2rppaU33la222q8n/HjNmpZmucdzgBwCAmOp6jf7wuNAQAArsGfuWMr51//evrV0quk30uWe9uB8lJ/e3+p37Lca1nuJyv9cfuO/UFtT3lx6TWXHvd7l9r/8EdwDABXJjgGAACgc/fd3nLf7etdlwEAjPkbKwAAAAAAEwTHAAAAAABMEBwDAAAAADDBHMdTdvz48TzzzDNdlwEAAAAA7MPx48dz8uTJrss4cATHU3by5EkdDQAAAAA41ExVAQAAAADABMExAAAAAAATqrU2/Tet+n9Jvjb1Nz4cbknyStdFcF3S9+iS/kdX9D26ou/RJf2Pruh7dEXfo0vXQ//74621Wy9vnElwfD2rqidaa/d3XQfXH32PLul/dEXfoyv6Hl3S/+iKvkdX9D26dD33P1NVAAAAAAAwQXAMAAAAAMAEwfH0PdJ1AVy39D26pP/RFX2Pruh7dEn/oyv6Hl3R9+jSddv/zHEMAAAAAMAEdxwDAAAAADBBcDwlVfWBqvpKVZ2tqp/uuh4WW1XdWVX/paq+VFXPVNXfGrf/XFW9UFVfHP/8UNe1sniq6rmqemrcx54Yt52oqs9X1VfH25u7rpPFUlV/asfY9sWq+oOq+knjHrNSVR+vqper6ukdbbuOdbXtn4+vA/9PVd3XXeUcdnv0vV+qqi+P+9ejVXXTuP3uqrq4Ywz8l91VziLYo//tea6tqp8Zj31fqar3d1M1i2CPvvfrO/rdc1X1xXG7sY+puUK+4rovpqqYiqrqJ/m/SX4wybkkjyf5cGvtS50WxsKqqtuT3N5ae7Kqjic5k+TPJ/lLSVZba/+k0wJZaFX1XJL7W2uv7Gj7xSTnW2u/MP7j2c2ttb/XVY0stvF594Uk703yYzHuMQNV9T1JVpP8m9baPeO2Xce6cYjy0SQ/lO1++c9aa+/tqnYOtz363vuS/OfW2lZV/eMkGfe9u5P8h0vPg7dqj/73c9nlXFtV707yqSQPJHl7kv+U5Ntaa8O5Fs1C2K3vXfbvv5zkm621nzf2MU1XyFd+NK773HE8JQ8kOdtae7a1tpHk00ke6rgmFlhr7cXW2pPj/deS/E6SO7qtiuvcQ0k+Md7/RLZPtDArfzbJ77bWvtZ1ISyu1tpvJzl/WfNeY91D2f6g21prX0hy0/hDCFy13fpea+1zrbWt8cMvJHnH3AvjurDH2LeXh5J8urW23lr7vSRns/3ZGK7alfpeVVW2b5L61FyL4rpwhXzFdV8Ex9NyR5Lndzw+FyEeczL+a+t3JPkf46aPjL8u8XHTBTAjLcnnqupMVT08bruttfbieP/3k9zWTWlcJz6UyQ8Oxj3mZa+xzrUg8/TXk/zHHY/fWVX/q6r+a1V9d1dFsfB2O9ca+5iX707yUmvtqzvajH1M3WX5iuu+CI7hUKuqY0n+bZKfbK39QZJfSfInk3x7kheT/HKH5bG4vqu1dl+SDyb5ifHXyt7QtudAMg8SM1FVy0l+OMlnxk3GPTphrKMLVfUPkmwl+bVx04tJ7mqtfUeSv53kk1V1Y1f1sbCca+nahzN504Cxj6nbJV95w/V83Sc4no4Xkty54/E7xm0wM1W1lO1B7ddaa/8uSVprL7XWhq21UZJ/HV8VYwZaay+Mty8neTTb/eylS1/PGW9f7q5CFtwHkzzZWnspMe4xd3uNda4Fmbmq+tEkfy7JXxl/gM14ioBXx/tnkvxukm/rrEgW0hXOtcY+Zq6qBkn+YpJfv9Rm7GPadstX4rovieB4Wh5P8q6qeuf4TqgPJXms45pYYOM5nn41ye+01v7pjvad8+r8hSRPX/5aeCuq6uh4wYBU1dEk78t2P3ssyY+Mn/YjSf59NxVyHZi448S4x5ztNdY9luSvjVfZPp3txXte3O0N4FpU1QeS/N0kP9xau7Cj/dbxgqGpqj+R5F1Jnu2mShbVFc61jyX5UFWtVNU7s93//ue862Ph/UCSL7fWzl1qMPYxTXvlK3HdlyQZdF3AIhivbvyRJL+VpJ/k4621Zzoui8X2nUn+apKnquqL47a/n+TDVfXt2f4KxXNJ/mY35bHAbkvy6Pa5NYMkn2yt/WZVPZ7kN6rqbyT5WrYXr4CpGv+x4gczObb9onGPWaiqTyX5viS3VNW5JD+b5Bey+1j32WyvrH02yYUkPzb3glkYe/S9n0mykuTz43PwF1prP57ke5L8fFVtJhkl+fHW2n4XNoM/Yo/+9327nWtba89U1W8k+VK2p1D5idbasIu6Ofx263uttV/NH13bIjH2MV175Suu+5LU+FtOAAAAAACQxFQVAAAAAABcRnAMAAAAAMAEwTEAAAAAABMExwAAAAAATBAcAwAAAAAwQXAMAAAAAMAEwTEAAAAAABMExwAAAAAATPj/g3D1pculRkQAAAAASUVORK5CYII=\n",
      "text/plain": [
       "<Figure size 1800x720 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "file_path = os.path.join(os.getcwd(), 'ZAM_Tutorial-1_2_T-1.xml')\n",
    "\n",
    "scenario, planning_problem_set = CommonRoadFileReader(file_path).open()\n",
    "\n",
    "# plot the scenario for each time step\n",
    "for i in range(0, 40):\n",
    "    display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(25, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  },
  "pycharm": {
   "stem_cell": {
    "cell_type": "raw",
    "metadata": {
     "collapsed": false
    },
    "source": []
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
